/* implemantation of BRIDGE function
    update: 1.6.11 14:20
msg completed:
*
*
*

TODO:
~  create real heart beat- with WD timer. (automatic option avialbe)
*  find where (in QGC) we enter DEBUG demand- called : request_MK_Debug
*  add more details to OSD- recived from MK (inside processMikroKopterCommand):
    V  add define PI -Done
    V  add vx,vy,vz to msg mavlink_msg_global_position_pack
    ~  check what is the timestamp onboard NC or FC - flytime_timestamp ??
    *  what is the rollspeed, nickspeed, yawspeed of NC
    V  use  RC_CHANNELS_SCALED (#36) for RSSI from NC
    *  use RAW_PRESSURE (#29) for navidata.Altimeter from NC
    X  use POSITION_TARGET (#63) for navidata.TargetPosition from NC --> may be problem with x,y - error!!
*  add prefix to the MK structs.
*  fix problem with acknoledge in writing WayPoints:
    V  may be in "sleep" function and thread managment- "sleep" was changed into "this->wait" and stops only current therad
    V  fix compering of current_WP (global variable) and wp_count (MK answer)
    V  check if number of points is limited by NC- checks result- yes, up to 32 points. check on MK-Tool 1.74a
    V  make writing WP - up to number of points limited be NC (32)- works good with serial connetion
    ~  make writing WP - up to number of points limited be NC (32)- works good with modem connetion- not working well
        after some time with no response or acknowlagde from UMV the QGC stops proccess
       V    create MK_OSD_INTERVAL variable that changes when WP is sent- give WP priority over OSD
    V  QGC is sending point again after inverval of time- no need to send it again

*  add limitation on number of WP (up to 31 as in MK-Tool 1.74a)

*/

#include "cbridge.h"

CBridge::CBridge(quint16 _receiver_port, quint16 _sender_port, const char *_serial_port_name,int _fullCapacity):QObject()
{
    serial_port = new QextSerialPort(_serial_port_name);

    receiver_port = _receiver_port;
    sender_port = _sender_port;

    total_WP = 0;
	//used for reading WPs
	first_read = 0;
    WatchDog_Counter = 0;
    CurrentOSDInterval = MK_OSD_INTERVAL;
    //stopping OSD while WP list is sent
    stop_OSD = 0;

    //creating MK Home variable- global to the bridge
    this->home_alt = 0;
    this->home_lat = 0;
    this->home_long = 0;
    this->home_status = 0;

    this->fullCapacity = _fullCapacity;
    EnablePrintFlags = false;
}

void CBridge::StartListening()
{
    serial_port->setBaudRate(BAUD57600);
    serial_port->setFlowControl(FLOW_XONXOFF);
    serial_port->setParity(PAR_NONE);
    serial_port->setDataBits(DATA_8);
    serial_port->setStopBits(STOP_1);
    serial_port->setTimeout(1000);
    serial_port->open(QIODevice::ReadWrite);

    slen = sizeof(si_other);

    if ((socket_id=socket(AF_INET, SOCK_DGRAM, IPPROTO_UDP))==-1)
    {
        perror("socket");
        exit(1);
    }
    memset((char *) &si_other, 0, sizeof(si_other));
    si_other.sin_family = AF_INET;
    si_other.sin_port = htons(sender_port);
    if (inet_aton(SRV_IP, &si_other.sin_addr)==0)
    {
        fprintf(stderr, "inet_aton() failed\n");
        exit(1);
    }

    // flush buffer.
    serial_port->readAll();

    SendMagicCode();

    // starts serial listening thread.
    pthread_create(&Serial_Receiver_Thread_ID,NULL,Serial_Receiver_Thread,this);
    qDebug() << "# Listening to serial port. OK.";

    // starts UDP listening thread.
    pthread_create(&UDP_Receiver_Thread_ID,NULL,UDP_Receiver_Thread,this);
    qDebug() << "# Listening to UDP socket. OK.";

    // starts UDP listening thread.
    pthread_create(&HeartBeat_Thread_ID,NULL,HeartBeat_Thread,this);
    qDebug() << "# Sending Heartbeat to QGC. OK.";

    CRC_ok = 0;
    CRC_fail = 0;

}


CBridge::~CBridge()
{
    SendMagicCode();

    serial_port->close();

    delete (serial_port);
    close(socket_id);

    pthread_join(Serial_Receiver_Thread_ID,NULL);
    pthread_join(UDP_Receiver_Thread_ID,NULL);
    pthread_join(HeartBeat_Thread_ID,NULL);
}

void CBridge::readPendingSerialData()
 {
    int i;

    serial_port->waitForReadyRead(-1);

    /* Check if bytes are available */
    if(serial_port && serial_port->isOpen() && serial_port->isWritable())
    {

        qint64 available = serial_port->bytesAvailable();

        if(available > 0)
        {

            if(MAX_RX_BUFFER < available) available = MAX_RX_BUFFER;

            // += means append
            serial_rx_buffer += serial_port->read(available);

            if (serial_rx_buffer.contains('\r'))
            {
                QList <QByteArray> segment = serial_rx_buffer.split('\r');

                for (i = 0 ; i < (segment.count() - 1) ; i++)
                {
                    if (!segment[i].isEmpty())
                    {
                        if (!segment[i].isEmpty())
                            processMikroKopterCommand(segment[i]);
                    }
                }

                serial_rx_buffer = segment[i];
            }
        }

    }
}


//reciveMK & MK2QGC
void CBridge::processMikroKopterCommand(QByteArray command)
{
    int len;
    uint8_t num_waypoints;
    uint8_t waypoint_index;
    Waypoint_t wp; 
    uint32_t version_int;
    uint8_t num_of_satalites;
    float height,airspeed,grnd_speed,gas;
    int16_t heading;
    int16_t PPM_Array[11];
    int16_t PPM_Scaled_Array[11];

    const char Debug_text[][10] = {"AngleNick" , "AngleRoll" , "AccNick  " , "AccRoll  " , "YawGyro  " , "HeightVal" ,
                                   "AccZ     " , "Gas      " , "CompasVal" , "Volt_0.1V" , "ReciveLev" , "GyroComps" ,
                                   "Motor 1  " , "Motor 2  " , "Motor 3  " , "Motor 4  " , "empty    " , "empty    " ,
                                   "empty    " , "empty    " , "Servo    " , "Hovergas " , "Curnt0.1A" , "Cpcty_mAh" ,
                                   "empty    " , "empty    " , "empty    " , "empty    " , "I2C-Error" , "BL Limit " ,
                                   "GPS_Nick " , "GPS_Roll "};

    //case of writing text to console.


    if (command[0] != '#')
    {
        if (command.endsWith("\r"))
        {
            CRC_fail++;
        }
        else
        {
            qDebug() <<  "\r# Received message from MK: " << command.simplified().split('#').first();
            fflush(stdout);
            std::cout << "MK_QGC (" << CurrentUARTAddress << ")>>";
            fflush(stdout);

            if (sendto(socket_id, command.data_ptr(), command.count(), 0, (sockaddr *)&si_other, (socklen_t)  slen)==-1)
             {
                 perror("sendto()");
                 exit(1);
             }
        }

        return;
    }

    //if we get msg from MK- WatchDog Counter return to zero
    WatchDog_Counter = 0;

    //checking the  of the received msg from MK -if check failed stops process.
    //TODO: should return value of  failed

    if (!checkRxCRC(command.count()))
    {
        CRC_fail++;
        return;
    }
    CRC_ok++;
    //trasform the command into regular values (was encoded in base64)
    command = decode64(command);

    //trasformation section MK2QGC
    switch(command[1]-'a') // check for Slave Address
    {
        case FC_ADDRESS:
                switch(command[2])
                {

                    case 'D':

//                       qDebug() << "\r# Receives debug array from MK.";
//                       fflush(stdout);
//                       std::cout << "MK_QGC (" << CurrentUARTAddress << ")>>";
//                       fflush(stdout);

                       DebugOut_t debugout;
                       memcpy((unsigned char *)&debugout ,command.data() + 3, sizeof(debugout));


                       for (int i = 0 ; i < 32 ; i++ )
                       {
                           if ((i < 16) || (i > 19 && i < 24) || (i > 27)) //don't send empty values
                           {
                               mavlink_msg_named_value_float_pack(SYSTEM_ID,MAV_COMP_ID_IMU,&mavlink_msg_to_qgc,Debug_text[i],(float)debugout.Analog[i]);
                               len = mavlink_msg_to_send_buffer((uint8_t*)&udp_tx_buffer, &mavlink_msg_to_qgc);
                               if (sendto(socket_id, udp_tx_buffer, len, 0, (sockaddr *)&si_other, (socklen_t)  slen)==-1)
                               {
                                    perror("sendto()");
                                    exit(1);
                               }
                           }
                       }
                       break;

                    case 'V':
                        str_VersionInfo verinfo;
                        memcpy((unsigned char *)&verinfo ,command.data() + 3, sizeof(verinfo));
                        qDebug() << "\r# FC version SWMajor: "  << verinfo.SWMajor;
                        qDebug() << "# FC version SWMinor: "    << verinfo.SWMinor;
                        qDebug() << "# FC version ProtoMajor: " << verinfo.ProtoMajor;
                        qDebug() << "# FC version ProtoMinor: " << verinfo.ProtoMinor;
                        qDebug() << "# FC version SWPatch: "    << verinfo.SWPatch;
                        fflush(stdout);
                        std::cout << "MK_QGC (" << CurrentUARTAddress << ")>>";
                        fflush(stdout);
                        break;

                    case 'P':
                        memcpy((unsigned char *)&PPM_Array ,command.data() + 3, sizeof(PPM_Array));
                        //qDebug()<<"PPM msg arraived";
                        //qDebug()<<"PPM Array : "<< PPM_Array[0]<<","<<PPM_Array[1]<<","<<PPM_Array[2]<<","<<PPM_Array[3]<<","<<PPM_Array[4]<<","<<PPM_Array[5]<<","<<PPM_Array[6]<<","<<PPM_Array[7];
                        //qDebug()<<"RC_Q : "<< RC_Q;

                        //scaling PPM
                        PPM_Scaled_Array[0] =  (PPM_Array[0] + 9)   * (20000 / (0 + 9))     - 10000 ;
                        PPM_Scaled_Array[1] =  (PPM_Array[1] + 123) * (20000 / (125 + 123)) - 10000 ;
                        PPM_Scaled_Array[2] = ((PPM_Array[2] + 126) * (20000 / (122 + 126)) - 10000) * (-1);
                        PPM_Scaled_Array[3] =  (PPM_Array[3] + 124) * (20000 / (123 + 124)) - 10000 ;
                        PPM_Scaled_Array[4] = ((PPM_Array[4] + 121) * (20000 / (127 + 121)) - 10000) * (-1);
                        PPM_Scaled_Array[5] = ((PPM_Array[5] + 122) * (20000 / (127 + 122)) - 10000) * (-1);
                        PPM_Scaled_Array[6] =  (PPM_Array[6] + 123) * (20000 / (128 + 123)) - 10000 ;
                        PPM_Scaled_Array[7] =  (PPM_Array[7] + 122) * (20000 / (128 + 122)) - 10000 ;


                        mavlink_msg_rc_channels_scaled_pack(SYSTEM_ID,MAV_COMP_ID_WAYPOINTPLANNER,&mavlink_msg_to_qgc,
                                PPM_Scaled_Array[0],PPM_Scaled_Array[1],PPM_Scaled_Array[2],PPM_Scaled_Array[3],PPM_Scaled_Array[4],PPM_Scaled_Array[5],PPM_Scaled_Array[6],PPM_Scaled_Array[7],RC_Q);
                        len = mavlink_msg_to_send_buffer((uint8_t*)&udp_tx_buffer, &mavlink_msg_to_qgc);
                        if (sendto(socket_id, udp_tx_buffer, len, 0, (sockaddr *)&si_other, (socklen_t)  slen)==-1)
                        {
                            perror("sendto()");
                            exit(1);
                        }
                        break;
                }
                break;
        case NC_ADDRESS:
                switch(command[2])
                {
/*
                    case 'Z':       //TODO- Serial Link Test ADDED
                        uint16_t EchoPattern;
                        memcpy((unsigned char *)&EchoPattern ,command.data() + 3, sizeof(EchoPattern));
                        if (EchoPattern == SentPatternLsb)
                            qDebug() << "Serial Link test is OK";
                        else
                            qDebug() << "Serial Link test is NOT OK";

                        //save current time ???
                        //???uint64_t recivedTime = timeGetTime();
                        QDateTime timestamp;
                        uint64_t recivedTime = (uint64)timestamp.setTime_t(unixTime);

                        mavlink_msg_ping_pack(SYSTEM_ID,MAV_COMP_ID_UART_BRIDGE,&mavlink_msg_to_qgc,sentSeq,sentTargetSystem,sentTargetComponent,recivedTime);
                        len = mavlink_msg_to_send_buffer((uint8_t*)&udp_tx_buffer, &mavlink_msg_to_qgc);
                        if (sendto(socket_id, udp_tx_buffer, len, 0, (sockaddr *)&si_other, (socklen_t)  slen)==-1)
                        {
                            perror("sendto()");
                            exit(1);
                        }
                        break;
*/
                    case 'E':
                        char error_text_msg[50];
                        memcpy((unsigned char *)&error_text_msg,command.data() + 3,sizeof(error_text_msg));
                        //qDebug()<< "ERROR Recived from MK: "<< error_text_msg;
                        mavlink_msg_statustext_pack(SYSTEM_ID,MAV_COMP_ID_SYSTEM_CONTROL,&mavlink_msg_to_qgc,CRITICAL_FAULT,(const int8_t*)error_text_msg);
                        len = mavlink_msg_to_send_buffer((uint8_t*)&udp_tx_buffer, &mavlink_msg_to_qgc);
                        if (sendto(socket_id, udp_tx_buffer, len, 0, (sockaddr *)&si_other, (socklen_t)  slen)==-1)
                         {
                             perror("sendto()");
                             exit(1);
                         }
                        break;


                    case 'W': // receive Number of WPs (proccess of writing to MK)
                        uint8_t wp_count;
                        memcpy((unsigned char *)&wp_count ,command.data() + 3, sizeof(wp_count));

                        // checking if the current WP is recived in MK properly

                        qDebug() << "\r# Receives waypoint count from MK. WP_COUNT: " << wp_count;
                        fflush(stdout);
                        std::cout << "MK_QGC (" << CurrentUARTAddress << ")>>";
                        fflush(stdout);

                        if (wp_count == current_WP)
                        {
                            if (current_WP < total_WP) // request next waypoint
                            {
                                qDebug() << "\r# Sends waypoint request to QGC. CURRENT_WP: " << current_WP;
                                fflush(stdout);
                                std::cout << "MK_QGC (" << CurrentUARTAddress << ")>>";
                                fflush(stdout);

                                mavlink_msg_waypoint_request_pack(SYSTEM_ID, MAV_COMP_ID_WAYPOINTPLANNER, &mavlink_msg_to_qgc,COMP_SYSTEM_ID,COMP_COMPONENT_ID,current_WP);
                                len = mavlink_msg_to_send_buffer((uint8_t*)&udp_tx_buffer, &mavlink_msg_to_qgc);
                                if (sendto(socket_id, udp_tx_buffer, len, 0, (sockaddr *)&si_other, (socklen_t)  slen)==-1)
                                 {
                                     perror("sendto()");
                                     exit(1);
                                 }

                            }
                            else // send acknowledge
                            {

                                qDebug() << "\r# Sends waypoint acknowladge to QGC.";
                                fflush(stdout);
                                std::cout << "MK_QGC (" << CurrentUARTAddress << ")>>";
                                fflush(stdout);

                                // the last parameter of "mavlink_msg_waypoint_ack_pack" is the type: 0 - OK, 1 - ERROR
                                mavlink_msg_waypoint_ack_pack(SYSTEM_ID, MAV_COMP_ID_WAYPOINTPLANNER, &mavlink_msg_to_qgc,COMP_SYSTEM_ID,COMP_COMPONENT_ID,0);
                                len = mavlink_msg_to_send_buffer((uint8_t*)&udp_tx_buffer, &mavlink_msg_to_qgc);
                                if (sendto(socket_id, udp_tx_buffer, len, 0, (sockaddr *)&si_other, (socklen_t)  slen)==-1)
                                 {
                                     perror("sendto()");
                                     exit(1);
                                 }

                                //WP list is over- OSD is back to high speed
                                CurrentOSDInterval = MK_OSD_INTERVAL;
                                //return OSD to work
                                stop_OSD = 0;
                            }
                        }
                        break;
                    case 'V': // TODO: check
                        str_VersionInfo verinfo;
                        memcpy((unsigned char *)&verinfo ,command.data() + 3, sizeof(verinfo));
                        qDebug() << "\r# NC version SWMajor: "    << verinfo.SWMajor;
                        qDebug() << "# NC version SWMinor: "    << verinfo.SWMinor;
                        qDebug() << "# NC version ProtoMajor: " << verinfo.ProtoMajor;
                        qDebug() << "# NC version ProtoMinor: " << verinfo.ProtoMinor;
                        qDebug() << "# NC version SWPatch: "    << verinfo.SWPatch;
                        fflush(stdout);
                        std::cout << "MK_QGC (" << CurrentUARTAddress << ")>>";
                        fflush(stdout);

                        break;
                    case 'O':
                        NaviData_t navidata;
                        uint16_t voltageRemaining;
                        float milivolts;
                        float lon,target_lon;
                        float lat,target_lat;
                        float alt,target_alt;
                        float vx,vy,vz;
                        float radRoll, radNick, radYaw;
                        uint16_t WP_current_seq;
                        uint64_t flytime_timestamp;
                        float target_dis, target_bearing;
                        memcpy((unsigned char *)&navidata ,command.data() + 3, sizeof(navidata));

                        //qDebug() << "WAYPOINTS: " << navidata
/*
                        //navidata struct:
                          ----------------
                        float height = (float)navidata.Altimeter ; //heigth as taken from air pressure sensor-should be in meters
                        radRoll = (float)navidata.AngleRoll / 180 * PI;
                        radNick = -(float)navidata.AngleNick / 180 * PI;
                        radYaw =  (float)navidata.CompassHeading / 180 * PI;
                     //navidata.Current;     //doesn't exists
                        alt = (float)navidata.CurrentPosition.Altitude  / 1000;
                        lat = (float)navidata.CurrentPosition.Latitude  / 10000000;
                        lon = (float)navidata.CurrentPosition.Longitude / 10000000;
                     //navidata.CurrentPosition.Status;  //doesn't exists
                            //navidata.Errorcode;
                        flytime_timestamp = (uint64_t)navidata.FlyingTime * 1000000; //using FlyingTime as timestamp
                        float gas= (float)navidata.Gas;
                        vx = (float)navidata.GroundSpeed / 100 * cos((float)navidata.CompassHeading / 180 * PI);
                        vy = (float)navidata.GroundSpeed / 100 * sin((float)navidata.CompassHeading / 180 * PI);
                        radYaw =  (float)navidata.Heading / 180 * PI;       //doesn't give back values
                        this->home_alt = navidata.HomePosition.Altitude;
                        this->home_lat = navidata.HomePosition.Latitude;
                        this->home_long = navidata.HomePosition.Longitud;
                        this->home_status = navidata.HomePosition.Status;
                        navidata.HomePositionDeviation.Bearing;     //sending to Debug
                        navidata.HomePositionDeviation.Distance;    //sending to Debug
                        navidata.FCStatusFlags;                 // print on screen
                        navidata.FCStatusFlags2;                // print on screen
                        navidata.NCFlags;                       // print on screen
                        //navidata.OperatingRadius;
                        RC_Q = (uint8_t) floor(((double)navidata.RC_Quality-100) * 255 / 100) ;
                        uint8_t num_of_satalites = (uint8_t)navidata.SatsInUse;
                        //navidata.SetpointAltitude;
                        //navidata.TargetHoldTime;
                        traget_lon = (float)navidata.TargetPosition.Longitude / 10000000;
                        traget_lat = (float)navidata.TargetPosition.Latitude / 10000000;
                        traget_alt = (float)navidata.TargetPosition.Altitude / 1000;
                     //navidata.TargetPosition.Status;   //doesn't exists
                        //navidata.TargetPositionDeviation.Bearing;
                        //navidata.TargetPositionDeviation.Distance;
                        float airspeed = (float)navidata.TopSpeed / 100; //[cm/s] to [m/s]
                        milivolts = (float)navidata.UBat * 100;
                        voltageRemaining = ((fullCapacity - SAFTY_CAPACITY - (float)navidata.UsedCapacity) / fullCapacity * 1000);
                        vz = (float)navidata.Variometer;
                     //navidata.Version;
                        WP_current_seq = (uint16_t) navidata.WaypointIndex;
                        //navidata.WaypointNumber;
*/

                        height = (float) navidata.Altimeter ; //should be in meters
                        airspeed = (float)navidata.TopSpeed / 100; //[cm/s] to [m/s]
                        grnd_speed = (float)navidata.GroundSpeed / 100;
                        heading =  (int16_t)floor((double)navidata.Heading / 180 * PI);
                        gas = ((float)navidata.Gas -32.0) * (100.0 / (161.0 - 32.0));
                        vz = (float)navidata.Variometer;                // TODO- check for units
                        //qDebug()<<"Altimeter = "<< navidata.Altimeter<<", height = "<<height;
                        //qDebug()<<"TopSpeed = "<<navidata.TopSpeed<<", airspeed = "<<airspeed;
                        //qDebug()<<"GroundSpeed = "<<navidata.GroundSpeed <<", grnd_speed = "<<grnd_speed;
                        //qDebug()<<"navidata.Heading = "<< navidata.Heading <<", Heading = "<<heading;
                        //qDebug()<<"navidata.Gas = "<<navidata.Gas<<", gas = "<<gas;

                        //qDebug()<<"Variometer = "<<navidata.Variometer<<", vz = "<<vz;
                        //qDebug()<<"--------------------------------------------";
                        mavlink_msg_vfr_hud_pack(SYSTEM_ID,MAV_COMP_ID_SYSTEM_CONTROL,&mavlink_msg_to_qgc,airspeed,grnd_speed,heading,gas,height,vz);
                        len = mavlink_msg_to_send_buffer((uint8_t*)&udp_tx_buffer, &mavlink_msg_to_qgc);
                        if (sendto(socket_id, udp_tx_buffer, len, 0, (sockaddr *)&si_other, (socklen_t)  slen)==-1)
                         {
                             perror("sendto()");
                             exit(1);
                         }

                        // sends GPS current position TODO: timestamp usec
                        lon = (float)navidata.CurrentPosition.Longitude / 10000000;
                        lat = (float)navidata.CurrentPosition.Latitude / 10000000;
                        alt = (float)navidata.CurrentPosition.Altitude / 1000;
                        vx = grnd_speed * cos((float)navidata.CompassHeading / 180 * PI);    //speed in m/s
                        vy = grnd_speed * sin((float)navidata.CompassHeading / 180 * PI);    //speed in m/s
                        //vz - already calculated before
                        flytime_timestamp = (uint64_t)navidata.FlyingTime * 1000000; //using FlyingTime as timestamp

                        //qDebug() << "lon: " << navidata.CurrentPosition.Longitude;
                        //qDebug() << "lat: " << navidata.CurrentPosition.Latitude;
                        //qDebug() << "alt : " << navidata.CurrentPosition.Altitude;
                        //qDebug() << "ground speed: " << navidata.GroundSpeed; //only works with GPS input!!
                        //qDebug() << "vx speed: " << vx;
                        //qDebug() << "vy speed: " << vy;
                        //qDebug() << "vz: " << navidata.Variometer;
                        //qDebug() << "compass Heading \\ Yaw: " << navidata.CompassHeading;
                        //qDebug() << "Heading \\ Yaw: " << navidata.Heading;
                        //qDebug() << "flying time: " << navidata.FlyingTime;
                        //qDebug() << "flytime_timestamp: " << flytime_timestamp;

                        mavlink_msg_global_position_pack(SYSTEM_ID,MAV_COMP_ID_GPS,&mavlink_msg_to_qgc,flytime_timestamp,lat,lon,alt,vx,vy,vz);
                        len = mavlink_msg_to_send_buffer((uint8_t*)&udp_tx_buffer, &mavlink_msg_to_qgc);
                        if (sendto(socket_id, udp_tx_buffer, len, 0, (sockaddr *)&si_other,(socklen_t)  slen)==-1)
                         {
                             perror("sendto()");
                             exit(1);
                         }

                        // sends Nick, Roll, Yaw TODO: set rollspeed, nickspeed, yawspeed and timestamp usec

                        radRoll = (float)navidata.AngleRoll / 180 * PI;
                        radNick = -(float)navidata.AngleNick / 180 * PI;
                        radYaw =  (float)navidata.CompassHeading / 180 * PI;

                        //qDebug() << "Roll: " << navidata.AngleRoll;
                        //qDebug() << "Nick: " << navidata.AngleNick;
                        //qDebug() << "Heading \\ Yaw: " << navidata.Heading;

                        mavlink_msg_attitude_pack(SYSTEM_ID,MAV_COMP_ID_IMU,&mavlink_msg_to_qgc,flytime_timestamp,radRoll,radNick,radYaw,0,0,0);
                        len = mavlink_msg_to_send_buffer((uint8_t*)&udp_tx_buffer, &mavlink_msg_to_qgc);
                        if (sendto(socket_id, udp_tx_buffer, len, 0, (sockaddr *)&si_other, (socklen_t)  slen)==-1)
                         {
                             perror("sendto()");
                             exit(1);
                         }

                        // sends Battary Status
                        milivolts = (float)navidata.UBat * 100; //UBat is in 0.1V steps
                        voltageRemaining = (int16_t) floor ((fullCapacity - SAFTY_CAPACITY - (double)navidata.UsedCapacity) / fullCapacity * 1000);
                        //qDebug() << "UBat: " << navidata.UBat;
                        //qDebug() << "UsedCapacity: " << navidata.UsedCapacity;
                        //qDebug() << "navidata.Errorcode"<<navidata.Errorcode;
                        //qDebug() << "MKFlags"<<navidata.FCStatusFlags;
                        //qDebug() << "MKFlags2"<<navidata.FCStatusFlags2;
                        //qDebug() << "NCFlags"<<navidata.NCFlags;
                        //qDebug() << "----------------------";

                        PrintFlags(navidata.FCStatusFlags, navidata.FCStatusFlags2, navidata.NCFlags);
                        EnablePrintFlags = false;



                        uint8_t mav_status;
                        mav_status = MAV_STATE_ACTIVE;
                        if (navidata.FCStatusFlags & FC_STATUS_LOWBAT)
                        {
                            mav_status = MAV_STATE_EMERGENCY;   //case of emergency
                            voltageRemaining = 0 ;      //working - but is it needed?
                            qDebug()<<"\r# Error: FC_STATUS_LOWBAT = "<< FC_STATUS_LOWBAT;
                            qDebug()<<"\r# Change QGC mav_status to MAV_STATE_EMERGENCY.";
                            fflush(stdout);
                            std::cout << "MK_QGC (" << CurrentUARTAddress << ")>>";
                            fflush(stdout);

                            char error_text_msg[50] = "Warning- LOW BATTERY !!! ";
                            mavlink_msg_statustext_pack(SYSTEM_ID,MAV_COMP_ID_SYSTEM_CONTROL,&mavlink_msg_to_qgc,CRITICAL_FAULT,(const int8_t*)error_text_msg);
                            len = mavlink_msg_to_send_buffer((uint8_t*)&udp_tx_buffer, &mavlink_msg_to_qgc);
                            if (sendto(socket_id, udp_tx_buffer, len, 0, (sockaddr *)&si_other, (socklen_t)  slen)==-1)
                             {
                                 perror("sendto()");
                                 exit(1);
                             }
                        }
                        if (navidata.FCStatusFlags & FC_STATUS_EMERGENCY_LANDING)
                        {
                            mav_status = MAV_STATE_EMERGENCY;   //case of emergency
                            qDebug()<<"\r# Error: FC_STATUS_EMERGENCY_LANDING = "<< FC_STATUS_EMERGENCY_LANDING;
                            qDebug()<<"\r# Change QGC mav_status to MAV_STATE_EMERGENCY.";
                            fflush(stdout);
                            std::cout << "MK_QGC (" << CurrentUARTAddress << ")>>";
                            fflush(stdout);

                            char error_text_msg[50] = "Warning- EMERGENCY LANDING !!! ";
                            mavlink_msg_statustext_pack(SYSTEM_ID,MAV_COMP_ID_SYSTEM_CONTROL,&mavlink_msg_to_qgc,CRITICAL_FAULT,(const int8_t*)error_text_msg);
                            len = mavlink_msg_to_send_buffer((uint8_t*)&udp_tx_buffer, &mavlink_msg_to_qgc);
                            if (sendto(socket_id, udp_tx_buffer, len, 0, (sockaddr *)&si_other, (socklen_t)  slen)==-1)
                             {
                                 perror("sendto()");
                                 exit(1);
                             }
                        }

                        mavlink_msg_sys_status_pack(SYSTEM_ID,MAV_COMP_ID_IMU,&mavlink_msg_to_qgc,MAV_MODE_AUTO, MAV_NAV_GROUNDED,mav_status,0,milivolts,voltageRemaining,0);
                        len = mavlink_msg_to_send_buffer((uint8_t*)&udp_tx_buffer, &mavlink_msg_to_qgc);
                        if (sendto(socket_id, udp_tx_buffer, len, 0, (sockaddr *)&si_other, (socklen_t)  slen)==-1)
                         {
                             perror("sendto()");
                             exit(1);
                         }

                        //sending current WP- added. ofir
                        WP_current_seq = (uint16_t) navidata.WaypointIndex;

                        mavlink_msg_waypoint_current_pack(SYSTEM_ID,MAV_COMP_ID_WAYPOINTPLANNER,&mavlink_msg_to_qgc,WP_current_seq);
                        len = mavlink_msg_to_send_buffer((uint8_t*)&udp_tx_buffer, &mavlink_msg_to_qgc);
                        if (sendto(socket_id, udp_tx_buffer, len, 0, (sockaddr *)&si_other, (socklen_t)  slen)==-1)
                         {
                             perror("sendto()");
                             exit(1);
                         }


                        //sending RC_Qualitiy:receiver signal strengh indicator.
                        if (navidata.RC_Quality >=200) //more then 100%
                            RC_Q = 255;
                        else
                            RC_Q = (uint8_t) floor(((double)navidata.RC_Quality-100) * 255 / 100) ;

                        //qDebug()<<"RC_Quality = "<<navidata.RC_Quality;
                        //qDebug()<<"RC_Q  = "<<RC_Q;

                        mavlink_msg_rc_channels_scaled_pack(SYSTEM_ID,MAV_COMP_ID_WAYPOINTPLANNER,&mavlink_msg_to_qgc,
                                PPM_Array[0] * 80,PPM_Array[1]* 80,PPM_Array[2]* 80,PPM_Array[3]* 80,PPM_Array[4]* 80,PPM_Array[5]* 80,PPM_Array[6]* 80,PPM_Array[7]* 80,RC_Q);
                        len = mavlink_msg_to_send_buffer((uint8_t*)&udp_tx_buffer, &mavlink_msg_to_qgc);
                        if (sendto(socket_id, udp_tx_buffer, len, 0, (sockaddr *)&si_other, (socklen_t)  slen)==-1)
                         {
                             perror("sendto()");
                             exit(1);
                         }

                        num_of_satalites = (uint8_t)navidata.SatsInUse;
                        uint8_t sats_prn[20]; //random numbers- not real sats info
                        uint8_t sats_in_use[20];
                        uint8_t sats_elevation[20];
                        uint8_t sats_direction[20];
                        uint8_t sats_azimut[20];
                        uint8_t sats_snr[20];
                        for (int i = 0 ; i < 20; i++)
                        {
                            sats_prn[i] = i + 1;     //random numbers- not real sats info
                            sats_elevation[i] = 0;   //random values- not real sats info
                            sats_direction[i] = 0;   //random values- not real sats info
                            sats_snr[i] = 100;       //random values- not real sats info
                            sats_azimut[i] = 0 ;     //random values- not real sats info
                            if (i < num_of_satalites)
                                sats_in_use[i]= 1; //satallite in use;
                            else
                                sats_in_use[i]= 0; //satallite NOT in use;
                        }
                        mavlink_msg_gps_status_pack(SYSTEM_ID,MAV_COMP_ID_WAYPOINTPLANNER,&mavlink_msg_to_qgc,num_of_satalites,
                                             (const int8_t*)sats_prn,(const int8_t*)sats_in_use,(const int8_t*)sats_elevation,(const int8_t*)sats_azimut,(const int8_t*)sats_snr);
                        len = mavlink_msg_to_send_buffer((uint8_t*)&udp_tx_buffer, &mavlink_msg_to_qgc);
                        if (sendto(socket_id, udp_tx_buffer, len, 0, (sockaddr *)&si_other, (socklen_t)  slen)==-1)
                        {
                             perror("sendto()");
                             exit(1);
                        }

                        this->home_alt = navidata.HomePosition.Altitude;
                        this->home_lat = navidata.HomePosition.Latitude;
                        this->home_long = navidata.HomePosition.Longitude;
                        this->home_status = navidata.HomePosition.Status;

                        //qDebug()<< "home_alt =  "<<(float)navidata.HomePosition.Altitude / 1000.0;
                        //qDebug()<< "home_lat =  "<<(float)navidata.HomePosition.Latitude / 10000000.0;
                        //qDebug()<< "home_long =  "<<(float)navidata.HomePosition.Longitude / 10000000.0;
                        //qDebug()<< "HomePositionDeviation.Bearing = " << (float) navidata.HomePositionDeviation.Bearing;
                        //qDebug()<< "HomePositionDeviation.Distance = " << (float) navidata.HomePositionDeviation.Distance;
                        //qDebug()<<"----------------------------------";

                        //sending values through DEBUG msg

                        mavlink_msg_named_value_float_pack(SYSTEM_ID,MAV_COMP_ID_WAYPOINTPLANNER,&mavlink_msg_to_qgc,"home.altitude",(float)this->home_alt / 1000.0);
                        len = mavlink_msg_to_send_buffer((uint8_t*)&udp_tx_buffer, &mavlink_msg_to_qgc);
                        if (sendto(socket_id, udp_tx_buffer, len, 0, (sockaddr *)&si_other, (socklen_t)  slen)==-1)
                         {
                             perror("sendto()");
                             exit(1);
                         }
                        mavlink_msg_named_value_float_pack(SYSTEM_ID,MAV_COMP_ID_WAYPOINTPLANNER,&mavlink_msg_to_qgc,"home.latitude",(float)this->home_lat / 10000000.0);
                        len = mavlink_msg_to_send_buffer((uint8_t*)&udp_tx_buffer, &mavlink_msg_to_qgc);
                        if (sendto(socket_id, udp_tx_buffer, len, 0, (sockaddr *)&si_other, (socklen_t)  slen)==-1)
                         {
                             perror("sendto()");
                             exit(1);
                         }
                        mavlink_msg_named_value_float_pack(SYSTEM_ID,MAV_COMP_ID_WAYPOINTPLANNER,&mavlink_msg_to_qgc,"home.longitude",(float)this->home_long / 10000000.0);
                        len = mavlink_msg_to_send_buffer((uint8_t*)&udp_tx_buffer, &mavlink_msg_to_qgc);
                        if (sendto(socket_id, udp_tx_buffer, len, 0, (sockaddr *)&si_other, (socklen_t)  slen)==-1)
                         {
                             perror("sendto()");
                             exit(1);
                         }

                        mavlink_msg_named_value_float_pack(SYSTEM_ID,MAV_COMP_ID_WAYPOINTPLANNER,&mavlink_msg_to_qgc,"home.Bearing",(float)navidata.HomePositionDeviation.Bearing);
                        len = mavlink_msg_to_send_buffer((uint8_t*)&udp_tx_buffer, &mavlink_msg_to_qgc);
                        if (sendto(socket_id, udp_tx_buffer, len, 0, (sockaddr *)&si_other, (socklen_t)  slen)==-1)
                         {
                             perror("sendto()");
                             exit(1);
                         }

                        mavlink_msg_named_value_float_pack(SYSTEM_ID,MAV_COMP_ID_WAYPOINTPLANNER,&mavlink_msg_to_qgc,"home.Distance",(float)navidata.HomePositionDeviation.Distance / 10.0);
                        len = mavlink_msg_to_send_buffer((uint8_t*)&udp_tx_buffer, &mavlink_msg_to_qgc);
                        if (sendto(socket_id, udp_tx_buffer, len, 0, (sockaddr *)&si_other, (socklen_t)  slen)==-1)
                         {
                             perror("sendto()");
                             exit(1);
                         }

                        target_lon = (float)navidata.TargetPosition.Longitude / 10000000.0;
                        target_lat = (float)navidata.TargetPosition.Latitude / 10000000.0;
                        target_alt = (float)navidata.TargetPosition.Altitude / 1000.0;
                        target_dis = (float)navidata.TargetPositionDeviation.Distance / 10.0;
                        target_bearing = (float)navidata.TargetPositionDeviation.Bearing;

                        //TODO- QGC print error- can't decode msg no. 63 = position_target
//                        //TODO- could be problem with x,y values
//                        mavlink_msg_position_target_pack(SYSTEM_ID,MAV_COMP_ID_WAYPOINTPLANNER,&mavlink_msg_to_qgc,traget_lon,traget_lat,traget_alt,radYaw);
//                        len = mavlink_msg_to_send_buffer((uint8_t*)&udp_tx_buffer, &mavlink_msg_to_qgc);
//                        if (sendto(socket_id, udp_tx_buffer, len, 0, (sockaddr *)&si_other, (socklen_t)  slen)==-1)
//                        {
//                            perror("sendto()");
//                            exit(1);
//                        }

                        mavlink_msg_named_value_float_pack(SYSTEM_ID,MAV_COMP_ID_WAYPOINTPLANNER,&mavlink_msg_to_qgc,"target.alt",target_alt);
                        len = mavlink_msg_to_send_buffer((uint8_t*)&udp_tx_buffer, &mavlink_msg_to_qgc);
                        if (sendto(socket_id, udp_tx_buffer, len, 0, (sockaddr *)&si_other, (socklen_t)  slen)==-1)
                        {
                            perror("sendto()");
                            exit(1);
                        }
                        mavlink_msg_named_value_float_pack(SYSTEM_ID,MAV_COMP_ID_WAYPOINTPLANNER,&mavlink_msg_to_qgc,"target.lat",target_lat);
                        len = mavlink_msg_to_send_buffer((uint8_t*)&udp_tx_buffer, &mavlink_msg_to_qgc);
                        if (sendto(socket_id, udp_tx_buffer, len, 0, (sockaddr *)&si_other, (socklen_t)  slen)==-1)
                        {
                            perror("sendto()");
                            exit(1);
                        }
                        mavlink_msg_named_value_float_pack(SYSTEM_ID,MAV_COMP_ID_WAYPOINTPLANNER,&mavlink_msg_to_qgc,"target.lon",target_lon);
                        len = mavlink_msg_to_send_buffer((uint8_t*)&udp_tx_buffer, &mavlink_msg_to_qgc);
                        if (sendto(socket_id, udp_tx_buffer, len, 0, (sockaddr *)&si_other, (socklen_t)  slen)==-1)
                        {
                            perror("sendto()");
                            exit(1);
                        }

                        mavlink_msg_named_value_float_pack(SYSTEM_ID,MAV_COMP_ID_WAYPOINTPLANNER,&mavlink_msg_to_qgc,"tar.bearing",target_bearing);
                        len = mavlink_msg_to_send_buffer((uint8_t*)&udp_tx_buffer, &mavlink_msg_to_qgc);
                        if (sendto(socket_id, udp_tx_buffer, len, 0, (sockaddr *)&si_other, (socklen_t)  slen)==-1)
                        {
                            perror("sendto()");
                            exit(1);
                        }

                        mavlink_msg_named_value_float_pack(SYSTEM_ID,MAV_COMP_ID_WAYPOINTPLANNER,&mavlink_msg_to_qgc,"tar.distance",target_dis);
                        len = mavlink_msg_to_send_buffer((uint8_t*)&udp_tx_buffer, &mavlink_msg_to_qgc);
                        if (sendto(socket_id, udp_tx_buffer, len, 0, (sockaddr *)&si_other, (socklen_t)  slen)==-1)
                        {
                            perror("sendto()");
                            exit(1);
                        }

                        //TODO - check True meannig of version -
                        version_int = (uint32_t)navidata.Version;
                        //qDebug() << "OSD version is: " << navidata.Version;
/*                        mavlink_msg_boot_pack(SYSTEM_ID,MAV_COMP_ID_SYSTEM_CONTROL,&mavlink_msg_to_qgc,version_int);
                        len = mavlink_msg_to_send_buffer((uint8_t*)&udp_tx_buffer, &mavlink_msg_to_qgc);
                        if (sendto(socket_id, udp_tx_buffer, len, 0, (sockaddr *)&si_other, (socklen_t)  slen)==-1)
                         {
                             perror("sendto()");
                             exit(1);
                         }
*/
                        if (navidata.Errorcode != 0) //0->OK, otherwish ask for error msg from MK
                        {
                            if (CurrentUARTAddress != NC_ADDRESS)
                                SendMagicCode();    //first go back to NC_ADDRESS and then to FC_ADDRESS
                            sendMikroKopterMsg('e',NC_ADDRESS,0);
                            //qDebug()<<"sendMikroKopterMsg('e',NC_ADDRESS,0)";
                        }


                        mavlink_msg_waypoint_current_pack(SYSTEM_ID,MAV_COMP_ID_WAYPOINTPLANNER,&mavlink_msg_to_qgc,(uint16_t)navidata.WaypointIndex);
                        len = mavlink_msg_to_send_buffer((uint8_t*)&udp_tx_buffer, &mavlink_msg_to_qgc);
                        if (sendto(socket_id, udp_tx_buffer, len, 0, (sockaddr *)&si_other,(socklen_t)  slen)==-1)
                         {
                             perror("sendto()");
                             exit(1);
                         }

                        break;

                     case 'X':
                    // receives Waypoint- read from MK


                        memcpy((unsigned char *)&num_waypoints ,command.data() + 3, sizeof(num_waypoints));
                        memcpy((unsigned char *)&waypoint_index ,command.data() + 3 + sizeof(num_waypoints), sizeof(waypoint_index));
                        memcpy((unsigned char *)&wp ,command.data() + 3 + sizeof(num_waypoints) + sizeof(waypoint_index), sizeof(wp));


                        if (first_read == 1)
                        {
                            // sends mavlink_msg_clear_all
                            // mavlink_msg_waypoint_clear_all_pack(SYSTEM_ID,MAV_COMP_ID_WAYPOINTPLANNER,&mavlink_msg_to_qgc,COMP_SYSTEM_ID,COMP_COMPONENT_ID);
                            // len = mavlink_msg_to_send_buffer((uint8_t*)&udp_tx_buffer, &mavlink_msg_to_qgc);
                            // sender_socket->writeDatagram(udp_tx_buffer,len,QHostAddress::LocalHost, sender_port);
							first_read = 0;

                            qDebug() << "\r# Receives waypoint count from MK. WP_COUNT: " << num_waypoints;
                            fflush(stdout);
                            std::cout << "MK_QGC (" << CurrentUARTAddress << ")>>";
                            fflush(stdout);

                            qDebug() << "\r# Sends waypoint count to QGC. WP_COUNT: " << num_waypoints;
                            fflush(stdout);
                            std::cout << "MK_QGC (" << CurrentUARTAddress << ")>>";
                            fflush(stdout);

                            // sends mavlink_msg_waypoint_count
                            mavlink_msg_waypoint_count_pack(SYSTEM_ID,MAV_COMP_ID_WAYPOINTPLANNER,&mavlink_msg_to_qgc,COMP_SYSTEM_ID,COMP_COMPONENT_ID,num_waypoints);
                            len = mavlink_msg_to_send_buffer((uint8_t*)&udp_tx_buffer, &mavlink_msg_to_qgc);
                            if (sendto(socket_id, udp_tx_buffer, len, 0, (sockaddr *)&si_other, (socklen_t)  slen)==-1)
                             {
                                 perror("sendto()");
                                 exit(1);
                             }


                        }
                        else

                        {
                            qDebug() << "\r# Receives waypoint from MK. WP_INDEX: " << waypoint_index;
                            fflush(stdout);
                            std::cout << "MK_QGC (" << CurrentUARTAddress << ")>>";
                            fflush(stdout);

                            float qgc_y = (float)wp.Position.Longitude / 10000000;
                            float qgc_x = (float)wp.Position.Latitude / 10000000;
                            float qgc_z = (float)wp.Position.Altitude / 1000;
                            float qgc_param1 = (float)wp.HoldTime / 1000;
                            float qgc_param2 = (float)wp.ToleranceRadius;
                            //qDebug() <<"wp.Heading = " << wp.Heading ;
                            //qDebug() <<"Event_Flag = " << wp.Event_Flag ;
                            qDebug() << "\r# Sends waypoint to QGC. WP_INDEX: " << waypoint_index;
                            fflush(stdout);
                            std::cout << "MK_QGC (" << CurrentUARTAddress << ")>>";
                            fflush(stdout);
							
							//sending WP to QGC
                            mavlink_msg_waypoint_pack(SYSTEM_ID,MAV_COMP_ID_WAYPOINTPLANNER,&mavlink_msg_to_qgc,COMP_SYSTEM_ID,COMP_COMPONENT_ID,waypoint_index - 1,MAV_FRAME_GLOBAL,MAV_CMD_NAV_WAYPOINT,0,0,qgc_param1,qgc_param2,0,0,qgc_x,qgc_y,qgc_z);		
                            len = mavlink_msg_to_send_buffer((uint8_t*)&udp_tx_buffer, &mavlink_msg_to_qgc);		
                            if (sendto(socket_id, udp_tx_buffer, len, 0, (sockaddr *)&si_other, (socklen_t)  slen)==-1)		
                             {		
                                 perror("sendto()");		
                                 exit(1);		
                             }
                        }



                        break;

                    }
                break;
        case MK3MAG_ADDRESS:
                switch(command[2])
                {
                    case 'k': // receive of compass heading data - refer to this command as heartbeat
                        int16_t heading;
                        memcpy((unsigned char *)&heading ,command.data() + 3, sizeof(heading));
                        //qDebug() << "Heading value: " << heading << endl;
                        mavlink_msg_heartbeat_pack(SYSTEM_ID, MAV_COMP_ID_IMU, &mavlink_msg_to_qgc, MAV_QUADROTOR, MAV_AUTOPILOT_GENERIC);
                        len = mavlink_msg_to_send_buffer((uint8_t*)&udp_tx_buffer, &mavlink_msg_to_qgc);
                        if (sendto(socket_id, udp_tx_buffer, len, 0, (sockaddr *)&si_other, (socklen_t)  slen)==-1)
                         {
                             perror("sendto()");
                             exit(1);
                         }

                        break;
                    case 'V': // TODO: check
                        str_VersionInfo verinfo;
                        memcpy((unsigned char *)&verinfo ,command.data() + 3, sizeof(verinfo));
                        qDebug() << "\r# MK3MAG version SWMajor: "    << verinfo.SWMajor;
                        qDebug() << "# MK3MAG version SWMinor: "    << verinfo.SWMinor;
                        qDebug() << "# MK3MAG version ProtoMajor: " << verinfo.ProtoMajor;
                        qDebug() << "# MK3MAG version ProtoMinor: " << verinfo.ProtoMinor;
                        qDebug() << "# MK3MAG version SWPatch: "    << verinfo.SWPatch;
                        fflush(stdout);
                        std::cout << "MK_QGC (" << CurrentUARTAddress << ")>>";
                        fflush(stdout);
                        /* TODO- problem with creating one integer from 4 chars
                        QString version_str = verinfo.SWMajor + verinfo.ProtoMajor + verinfo.ProtoMinor + verinfo.ProtoMinor ;
                        bool work;
                        version_int = (uint32_t) version_str.toInt(&work,10);
                        if (work)   //conversion success
                        {
                            mavlink_msg_boot_pack(SYSTEM_ID,MAV_COMP_ID_SYSTEM_CONTROL,&mavlink_msg_to_qgc,version_int);
                            len = mavlink_msg_to_send_buffer((uint8_t*)&udp_tx_buffer, &mavlink_msg_to_qgc);
                            sender_socket->writeDatagram(udp_tx_buffer,len,QHostAddress::LocalHost, sender_port);
                        }
                        else
                            qDebug()<<"ERROR with conversion of version_str to integer";
                        break;
                        */
                        break;

                }
                break;
        case MKGPS_ADDRESS:
                switch(command[2])
                {
                    case 'V': // TODO: check
                        str_VersionInfo verinfo;
                        memcpy((unsigned char *)&verinfo ,command.data() + 3, sizeof(verinfo));
                        qDebug() << "\r# MKGPS version SWMajor: "    << verinfo.SWMajor;
                        qDebug() << "# MKGPS version SWMinor: "    << verinfo.SWMinor;
                        qDebug() << "# MKGPS version ProtoMajor: " << verinfo.ProtoMajor;
                        qDebug() << "# MKGPS version ProtoMinor: " << verinfo.ProtoMinor;
                        qDebug() << "# MKGPS version SWPatch: "    << verinfo.SWPatch;
                        fflush(stdout);
                        std::cout << "MK_QGC (" << CurrentUARTAddress << ")>>";
                        fflush(stdout);
                        break;
               }
            break;

    }

}

void CBridge::QGC2MK(mavlink_message_t msg)
{
    int len;
    uint8_t index;
    uint8_t engin_values1[16]= {10,10,10,10,0,0,0,0,0,0,0,0,0,0,0,0};
    switch (msg.msgid)
    {
        case MAVLINK_MSG_ID_COMMAND:    //msg "COMMAND" #75 --->TODO- could not see what trigger it

            switch(mavlink_msg_command_get_command(&msg))
            {
                case MAV_CMD_DO_SET_HOME:
                    qDebug()<<"can NOT set new location for HOME Position- using MK HOME";
                    mavlink_msg_command_ack_pack(SYSTEM_ID,MAV_COMP_ID_WAYPOINTPLANNER,&mavlink_msg_to_qgc,mavlink_msg_command_get_command(&msg),COMMAND_ACCEPTED_N_EXECUTED);
                    len = mavlink_msg_to_send_buffer((uint8_t*)&udp_tx_buffer, &mavlink_msg_to_qgc);
                    if (sendto(socket_id, udp_tx_buffer, len, 0, (sockaddr *)&si_other, (socklen_t)  slen)==-1)
                     {
                         perror("sendto()");
                         exit(1);
                     }
                    break;

                case MAV_CMD_NAV_LAND:  //should be lending at current location
                    qDebug()<< "Land at Current position - NOT WORKING YET";
                    break;

                case MAV_CMD_NAV_RETURN_TO_LAUNCH:
                    qDebug()<< "Return to Launch - NOT WORKING YET";
                    break;

                default:
                    qDebug()<<"QGC command unknowen or unsupported";
                    mavlink_msg_command_ack_pack(SYSTEM_ID,MAV_COMP_ID_WAYPOINTPLANNER,&mavlink_msg_to_qgc,mavlink_msg_command_get_command(&msg),COMMAND_UNKNOWEN_UNSUPPORTED);
                    len = mavlink_msg_to_send_buffer((uint8_t*)&udp_tx_buffer, &mavlink_msg_to_qgc);
                    if (sendto(socket_id, udp_tx_buffer, len, 0, (sockaddr *)&si_other, (socklen_t)  slen)==-1)
                     {
                         perror("sendto()");
                         exit(1);
                     }
                    break;
            }
            break;
        case MAVLINK_MSG_ID_ACTION:
            Waypoint_t target_wp;

            switch(mavlink_msg_action_get_action(&msg))
            {
                case MAV_ACTION_EMCY_LAND:
                    qDebug()<<"STARTING EMERGENCY LANDING TO HOME Position- using MK HOME";
                    target_wp.Position.Altitude = this->home_alt;
                    target_wp.Position.Latitude = this->home_lat;
                    target_wp.Position.Longitude = this->home_long;
                    target_wp.Position.Status = this->home_status;
                    target_wp.Heading = 0 ;     //TODO- always comes from north to land.
                    target_wp.Event_Flag = 0 ;  //TODO- FUTURE implematation
                    target_wp.HoldTime = HOME_HOLD_TIME ; //time in sec to stay in HOME position
                    target_wp.Index = 0 ;
                    for (int i = 0; i < 11; i++) target_wp.reserve[i] = 0;
                    target_wp.ToleranceRadius = HOME_TOLERANCE_RADIUS ;
                    sendMikroKopterMsg('s',NC_ADDRESS,1,&target_wp,sizeof(target_wp));

                    mavlink_msg_action_ack_pack(SYSTEM_ID,MAV_COMP_ID_WAYPOINTPLANNER,&mavlink_msg_to_qgc,MAV_ACTION_EMCY_LAND,MAV_ACTION_CONFIRMED);
                    len = mavlink_msg_to_send_buffer((uint8_t*)&udp_tx_buffer, &mavlink_msg_to_qgc);
                    if (sendto(socket_id, udp_tx_buffer, len, 0, (sockaddr *)&si_other, (socklen_t)  slen)==-1)
                     {
                         perror("sendto()");
                         exit(1);
                     }
                    break;

                case MAV_ACTION_RETURN:
                    qDebug()<<"RETURNNING TO HOME Position- using MK HOME";
                    target_wp.Position.Altitude = this->home_alt;
                    target_wp.Position.Latitude = this->home_lat;
                    target_wp.Position.Longitude = this->home_long;
                    target_wp.Position.Status = this->home_status;
                    target_wp.Heading = 0 ; //always comes from north to land.
                    target_wp.Event_Flag = 0 ;  //TODO- FUTURE implematation
                    target_wp.HoldTime = HOME_HOLD_TIME ; //time in sec to stay in HOME position
                    target_wp.Index = 0 ;
                    for (int i = 0; i < 11; i++) target_wp.reserve[i] = 0;
                    target_wp.ToleranceRadius = HOME_TOLERANCE_RADIUS ;
                    sendMikroKopterMsg('s',NC_ADDRESS,1,&target_wp,sizeof(target_wp));

                    mavlink_msg_action_ack_pack(SYSTEM_ID,MAV_COMP_ID_WAYPOINTPLANNER,&mavlink_msg_to_qgc,MAV_ACTION_RETURN,MAV_ACTION_CONFIRMED);
                    len = mavlink_msg_to_send_buffer((uint8_t*)&udp_tx_buffer, &mavlink_msg_to_qgc);
                    if (sendto(socket_id, udp_tx_buffer, len, 0, (sockaddr *)&si_other, (socklen_t)  slen)==-1)
                     {
                         perror("sendto()");
                         exit(1);
                     }
                    break;

                default:
                    qDebug()<<"QGC ACTION unknowen or unsupported";
                    mavlink_msg_action_ack_pack(SYSTEM_ID,MAV_COMP_ID_WAYPOINTPLANNER,&mavlink_msg_to_qgc,mavlink_msg_action_get_action(&msg),MAV_ACTION_DENIED);
                    len = mavlink_msg_to_send_buffer((uint8_t*)&udp_tx_buffer, &mavlink_msg_to_qgc);
                    if (sendto(socket_id, udp_tx_buffer, len, 0, (sockaddr *)&si_other, (socklen_t)  slen)==-1)
                    {
                         perror("sendto()");
                         exit(1);
                    }
                    break;
            }
            break;
        case MAVLINK_MSG_ID_SET_MODE:
            switch(mavlink_msg_set_mode_get_mode(&msg))
            {
                case MAV_MODE_TEST1:
                    qDebug()<<"starting engin test";
                    sendMikroKopterMsg('t',FC_ADDRESS,1,&engin_values1,sizeof(engin_values1));
                    break;

                case MAV_MODE_TEST2:
                    qDebug()<<"QGC MODE unknowen or unsupported";
                    break;

                default:
                    qDebug()<<"QGC MODE unknowen or unsupported";
                    break;
            }

            break;
        //PING command is not working yet
        case MAVLINK_MSG_ID_PING:       //TODO- command isn't found in QGC- not implemnted
            qDebug()<<"PING command is not supported yet";
            /*
            uint16_t SentPatternLsb;  //TODO - add  uint16_t SentPatternLsb to global var - to be checked later
            mavlink_ping_t sentPing;

            mavlink_msg_ping_decode(&msg,sentPing);
            uint64_t SentTime = sentPing.time;      //TODO- add uint64_t SentTime to global variables- to be checked later
            uint8_t sentTargetComponent = sentPing.target_component;
            uint8_t sentTargetSystem = sentPing.target_system;
            uint32_t sentSeq = sentPing.seq;
            //SentPatternLsb = (uint16_t) (0x0000FFFF & sentPing.seq); //is it working ???
            SentPatternLsb = (uint16_t) sentPing.seq;   //is it working ???
            sendMikroKopterMsg ('z',NC_ADDRESS,1,&SentPatternLsb,sizeof(uint16_t));
            */
            break;
        //QGC askes the number of WP stored in MK
        case MAVLINK_MSG_ID_WAYPOINT_REQUEST_LIST:

            qDebug() << "\r# Receives waypoint list request from QGC.";
            fflush(stdout);
            std::cout << "MK_QGC (" << CurrentUARTAddress << ")>>";
            fflush(stdout);
			//that is the first read of WPs
			first_read = 1;
            
			mavlink_waypoint_request_list_t wprlst;
            uint8_t i;

            mavlink_msg_waypoint_request_list_decode(&msg,&wprlst);

            // refer this command as get wp count.
            i = 1;

            qDebug() << "\r# Sends waypoint count request to MK.";
            fflush(stdout);
            std::cout << "MK_QGC (" << CurrentUARTAddress << ")>>";
            fflush(stdout);

            sendMikroKopterMsg('x',NC_ADDRESS,1,&i,sizeof(i));

            break;

        //QGC askes the next WP stored in MK
        case MAVLINK_MSG_ID_WAYPOINT_REQUEST:


            mavlink_waypoint_request_t wpr;
            mavlink_msg_waypoint_request_decode(&msg,&wpr);

            index = wpr.seq + 1;    //adjusment between QGC(WP starts with '0') and MK (WOP start with '1')


            qDebug() << "\r# Receives waypoint request from QGC. WP_INDEX: " << index;
            fflush(stdout);
            std::cout << "MK_QGC (" << CurrentUARTAddress << ")>>";
            fflush(stdout);

            qDebug() << "\r# Sends waypoint request to MK. WP_INDEX: " << index;
            fflush(stdout);
            std::cout << "MK_QGC (" << CurrentUARTAddress << ")>>";
            fflush(stdout);

            sendMikroKopterMsg('x',NC_ADDRESS,1,&index,sizeof(index));

            break;

        case MAVLINK_MSG_ID_WAYPOINT:

            qDebug() << "\r# Receives waypoint from QGC.";
            fflush(stdout);
            std::cout << "MK_QGC (" << CurrentUARTAddress << ")>>";
            fflush(stdout);

            SendWaypoint2MK(msg);
            break;

        case MAVLINK_MSG_ID_WAYPOINT_COUNT:
            // send waypoint msg using mavlink_msg_waypoint_decode() function
            mavlink_waypoint_count_t wpcount;
            // decoding waypoing struct from the messege
            mavlink_msg_waypoint_count_decode(&msg, &wpcount);

            total_WP = wpcount.count;

            // clears the WP list in MK (NaviCTRL)
            clear_MK_WP();

            // request waypoint 0
            mavlink_msg_waypoint_request_pack(SYSTEM_ID, MAV_COMP_ID_WAYPOINTPLANNER, &mavlink_msg_to_qgc,COMP_SYSTEM_ID,COMP_COMPONENT_ID, current_WP);
            len = mavlink_msg_to_send_buffer((uint8_t*)&udp_tx_buffer, &mavlink_msg_to_qgc);
            if (sendto(socket_id, udp_tx_buffer, len, 0, (sockaddr *)&si_other, (socklen_t)  slen)==-1)
            {
                 perror("sendto()");
                 exit(1);
            }

            break;

    }
}

//send2MK=sendMikroKopterMsg:
//	cmd- name command to be sent (one letter)
//	address- address of part in MK ('b'=FC, 'c'=NC, 'd'=MK3Mag)
//	args_no- number of arguments atteced- max of 3 arguments
//      argument specification is &(arg) and it's size (with sizeof(arg)
void CBridge::sendMikroKopterMsg(unsigned char cmd,unsigned char address, unsigned char args_no, ...)
{
    va_list ap;

    unsigned int pt = 0;
    unsigned char a,b,c;
    unsigned char ptr = 0;

    unsigned char *snd = 0;
    int len = 0;

    serial_tx_buffer[pt++] = '#';
    serial_tx_buffer[pt++] = 'a' + address;
    serial_tx_buffer[pt++] = cmd;

    va_start(ap, args_no);
    if(args_no)
    {
        snd = va_arg(ap, unsigned char*);
        len = va_arg(ap, int);
        ptr = 0;
        args_no--;
    }
    while(len)
    {
        if(len)
        {
                a = snd[ptr++];
                len--;
                if((!len) && args_no)
                {
                    snd = va_arg(ap, unsigned char*);
                    len = va_arg(ap, int);
                    ptr = 0;
                    args_no--;
                }
        }
    else a = 0;
    if(len)
    {
                b = snd[ptr++];
                len--;
        if((!len) && args_no)
        {
                        snd = va_arg(ap, unsigned char*);
            len = va_arg(ap, int);
            ptr = 0;
            args_no--;
                }
        }
    else b = 0;
    if(len)
        {
                c = snd[ptr++];
        len--;
        if((!len) && args_no)
        {
                        snd = va_arg(ap, unsigned char*);
                        len = va_arg(ap, int);
            ptr = 0;
            args_no--;
                }
    }
    else c = 0;
        serial_tx_buffer[pt++] = '=' + (a >> 2);
        serial_tx_buffer[pt++] = '=' + (((a & 0x03) << 4) | ((b & 0xf0) >> 4));
        serial_tx_buffer[pt++] = '=' + (((b & 0x0f) << 2) | ((c & 0xc0) >> 6));
        serial_tx_buffer[pt++] = '=' + ( c & 0x3f);
    }
    va_end(ap);
    addCRC(pt);

    pt += 5;

    serial_port->write(serial_tx_buffer,pt);
    serial_port->waitForBytesWritten(-1);

    }

void CBridge::addCRC(unsigned int current_length)
{
 unsigned int tmp = 0,i;
 for(i = 0; i < current_length;i++)
  {
   tmp += serial_tx_buffer[i];
  }
   tmp %= 4096;
   serial_tx_buffer[i++] = '=' + tmp / 64;
   serial_tx_buffer[i++] = '=' + tmp % 64;
   serial_tx_buffer[i++] = '\r';
   serial_tx_buffer[i++] = '\r';
   serial_tx_buffer[i++] = '\r';
}

//---------------------------------------------------------------------------------------------
// relevent commands QGC2MK
//---------------------------------------------------------------------------------------------
void CBridge::request_MK_Debug (uint8_t AutoSendInterval)
{
//    qDebug() << "\r# Sends debug request to MK.";
//    fflush(stdout);
//    std::cout << "MK_QGC (" << CurrentUARTAddress << ")>>";
//    fflush(stdout);

    sendMikroKopterMsg('d',ANY_ADDRESS,1,&AutoSendInterval,sizeof(AutoSendInterval));
}

void CBridge::request_MK_OSD (uint8_t AutoSendInterval)
{
    sendMikroKopterMsg('o',NC_ADDRESS,1,&AutoSendInterval,sizeof(AutoSendInterval));
}

void CBridge::request_MK_PPM_Channels()
{
    if (CurrentUARTAddress != FC_ADDRESS)
    {
        if (CurrentUARTAddress != NC_ADDRESS)
            SendMagicCode();    //first go back to NC_ADDRESS and then to FC_ADDRESS
        redirect_UART(FC_ADDRESS);
    }
    sendMikroKopterMsg('p',FC_ADDRESS,0);
}

//sending one way point data
void CBridge::SendWaypoint2MK(mavlink_message_t msg)
{
        //send waypoint msg using mavlink_msg_waypoint_decode() function
        mavlink_waypoint_t waypointQGC;
        //decoding waypoing struct from the messege
        mavlink_msg_waypoint_decode(&msg, &waypointQGC);


        Point_t waypointMK;

        waypointMK.Position.Latitude = waypointQGC.x * 10000000;
        waypointMK.Position.Longitude = waypointQGC.y * 10000000;
        waypointMK.Position.Altitude = waypointQGC.z * 1000;
        waypointMK.Position.Status = NEWDATA;
        waypointMK.Heading = -1;
        // TODO: find out
        // ToleranceRadius & HoldTime are compatible to the GUI and not to mavlink_waypoint_t struct description.
        waypointMK.ToleranceRadius = waypointQGC.param2;
        waypointMK.HoldTime = waypointQGC.param1 * 1000;
        waypointMK.Event_Flag = 0;
        waypointMK.Index = waypointQGC.seq + 1;

        // type means: 0: waypoint, 1: point fo interest
        if (waypointQGC.seq + 1 == 1)
            waypointMK.Type = 1;
        else
            waypointMK.Type = 0;
		
		//TODO- check if works without line below
        waypointMK.Type = 0;
        waypointMK.WP_EventChannelValue = 0;
        waypointMK.AltitudeRate = 0;

        for (int j = 0 ; j < 8 ; j++)
            waypointMK.reserve[j]=0;   // setting reserve to zero

        qDebug() << "\r# Sends waypoint to MK. WP_INDEX: "<< waypointQGC.seq + 1;
        fflush(stdout);
        std::cout << "MK_QGC (" << CurrentUARTAddress << ")>>";
        fflush(stdout);

        current_WP = waypointQGC.seq + 1;

        sendMikroKopterMsg('w',NC_ADDRESS,1,&waypointMK,sizeof(Point_t));

    }

void CBridge::clear_MK_WP()
{
    Point_t waypointMK;

    waypointMK.Position.Longitude = INVALID;
    waypointMK.Position.Latitude = INVALID;
    waypointMK.Position.Altitude = INVALID;
    waypointMK.Position.Status = INVALID;
    waypointMK.Heading = INVALID;
    waypointMK.ToleranceRadius = INVALID;
    waypointMK.HoldTime = INVALID;
    waypointMK.Event_Flag = INVALID;
    waypointMK.Index = INVALID;
    waypointMK.Type = INVALID;
    waypointMK.WP_EventChannelValue = INVALID;
    waypointMK.AltitudeRate = INVALID;

    for (int j = 0 ; j < 8 ; j++)
        waypointMK.reserve[j] = INVALID;

    sendMikroKopterMsg('w',NC_ADDRESS,1,&waypointMK,sizeof(Point_t));

    qDebug() << "\r# Sends INVALID waypoint (clear) to MK.";
    fflush(stdout);
    std::cout << "MK_QGC (" << CurrentUARTAddress << ")>>";
    fflush(stdout);

    //change the OSD time interval to give priority to WP list.
    CurrentOSDInterval = MK_OSD_INTERVAL * MK_OSD_FACTOR ;
    //stopping OSD while sending WP list
    stop_OSD = MK_OSD_STOP;
    current_WP = 0;
}

void CBridge::request_MK_version(int slave_address)
{
    qDebug() << "\r# Requests version from MK...";
    sendMikroKopterMsg('v',slave_address,0);
}

void CBridge::request_MK_Reset()
{
    qDebug() << "\r# Requests reset from MK...";
    sendMikroKopterMsg('R',ANY_ADDRESS,0);

    fflush(stdout);
    std::cout << "MK_QGC (" << CurrentUARTAddress << ")>>";
    fflush(stdout);
}

//send command to NC to redirect msg to FC or MK3MAG
void CBridge::redirect_UART(int slave_address)
{
    serial_port->write("\r\r\r", 3);

    char seq1[12] = {0x23,0x63,0x75,0x3D,0x43,0x62,0x52,0x45,0x6C,0x0D,0x0D,0x0D}; // Switch to FC
    char seq2[12] = {0x23,0x63,0x75,0x3D,0x53,0x62,0x52,0x45,0x7C,0x0D,0x0D,0x0D}; // Switch to MK3MAG
    char seq3[12] = {0x23,0x63,0x75,0x3D,0x5D,0x3D,0x3E,0x45,0x4D,0x0D,0x0D,0x0D}; // Switch to MKGPS


    switch (slave_address)
    {
    case FC_ADDRESS:
        qDebug() << "# Redirecting MK UART to FC...";
        serial_port->write(seq1, 12);
        serial_port->waitForBytesWritten(-1);
        CurrentUARTAddress = FC_ADDRESS;
        break;
    case MK3MAG_ADDRESS:
        qDebug() << "# Redirecting MK UART to MK3MAG...";
        serial_port->write(seq2, 12);
        serial_port->waitForBytesWritten(-1);
        CurrentUARTAddress = MK3MAG_ADDRESS;
        break;
    case MKGPS_ADDRESS:
        qDebug() << "# Redirecting MK UART to MKGPS...";
        qDebug() << "# This might take few seconds...";
        serial_port->write(seq3, 12);
        serial_port->waitForBytesWritten(-1);
        CurrentUARTAddress = MKGPS_ADDRESS;
        break;
    }

    serial_port->waitForReadyRead(-1);

    usleep(1000*300);

}

//send MagicCode to redirect to NC
void CBridge::SendMagicCode()
{

    qDebug() << "# Sends magic code to MK...";

    char seq[5] = {0x1B,0x1B,0x55,0xAA,0x00}; // Magic Code

    serial_port->write(seq, 5);
    serial_port->waitForBytesWritten(-1);

    CurrentUARTAddress = NC_ADDRESS;
    usleep(1000*300);
}

void CBridge::Goto(int address)
{
    if (address == FC_ADDRESS && CurrentUARTAddress != FC_ADDRESS)
    {
        if (CurrentUARTAddress != NC_ADDRESS)
            SendMagicCode();    //first go back to NC_ADDRESS and then to FC_ADDRESS
        redirect_UART(FC_ADDRESS);
    }

    if (address == MK3MAG_ADDRESS && CurrentUARTAddress != MK3MAG_ADDRESS)
    {
        if (CurrentUARTAddress != NC_ADDRESS)
            SendMagicCode();    //first go back to NC_ADDRESS and then to MK3MAG_ADDRESS
        redirect_UART(MK3MAG_ADDRESS);
    }

    if (address == MKGPS_ADDRESS && CurrentUARTAddress != MKGPS_ADDRESS)
    {
        if (CurrentUARTAddress != NC_ADDRESS)
            SendMagicCode();    //first go back to NC_ADDRESS and then to MK3MAG_ADDRESS
        redirect_UART(MKGPS_ADDRESS);
    }
    if (address == NC_ADDRESS && CurrentUARTAddress != NC_ADDRESS)
        SendMagicCode();
}

/*---------------------------------------------------------------------------------------------
process from MK to QGC:
MK-> reciveMK through serial port -> MK2QGC (with BRIDGE) -> send2QGC through UDP -> QGC
--------------------------------------------------------------------------------------------*/

void CBridge::Timer_Heartbeat_Tick()
{
    struct sockaddr_in si_other;
    int socket_id;
    int slen;
    slen = sizeof(si_other);

    if ((socket_id=socket(AF_INET, SOCK_DGRAM, IPPROTO_UDP))==-1)
    {
        perror("socket");
        exit(1);
    }
    memset((char *) &si_other, 0, sizeof(si_other));
    si_other.sin_family = AF_INET;
    si_other.sin_port = htons(sender_port);
    if (inet_aton(SRV_IP, &si_other.sin_addr)==0)
    {
        fprintf(stderr, "inet_aton() failed\n");
        exit(1);
    }

//    dataMutex.lock();
//adding non automatic option to HeartBeat
#ifdef WATCHDOG_ON
    WatchDog_Counter++;
    if (WatchDog_Counter < WATCHDOG_COUNTER_LIMIT)
    {
#endif
        // Sending HeartBeat to QGC
        mavlink_msg_heartbeat_pack(SYSTEM_ID, MAV_COMP_ID_IMU, &mavlink_msg_to_qgc, MAV_QUADROTOR, MAV_AUTOPILOT_GENERIC);
        int len = mavlink_msg_to_send_buffer((uint8_t*)&udp_tx_buffer, &mavlink_msg_to_qgc);
        if (sendto(socket_id, udp_tx_buffer, len, 0, (sockaddr *)&si_other, (socklen_t)  slen)==-1)
         {
             perror("sendto()");
             exit(1);
         }

        //dataMutex.unlock();
        if (!stop_OSD)
        {
            // Requesting OSD Data
            if (CurrentUARTAddress == NC_ADDRESS)
                request_MK_OSD(CurrentOSDInterval);
            if (CurrentUARTAddress == FC_ADDRESS)
                request_MK_Debug(CurrentOSDInterval);
        }
        //qDebug()<<"PPM counter ="<<counter;
        //if (CurrentUARTAddress == FC_ADDRESS)
        //    request_MK_PPM_Channels();


#ifdef WATCHDOG_ON
    }
    else
    {
        qDebug()<<"ERROR with WatchDog- no heartbeat from MK";
        //dataMutex.unlock();
        // Requesting OSD Data
        if (!stop_OSD)
        {
            request_MK_OSD(CurrentOSDInterval);
        }
    }
#endif
    close(socket_id);

}

bool CBridge::checkRxCRC(unsigned int current_length)
{
    unsigned int tmp = 0,i;
    for(i = 0; i < current_length - 2 ; i++)
     {
      tmp += serial_rx_buffer[i];
     }
      tmp %= 4096;
      if (serial_rx_buffer[i++] == (char)('=' + tmp / 64) && serial_rx_buffer[i++] == (char)('=' + tmp % 64))
          return true;
      else
          return false;
}

QByteArray CBridge::decode64(QByteArray command)
{
 unsigned char a,b,c,d;
 unsigned char x,y,z;
 unsigned char ptrIn = 3; // start at begin of data block
 unsigned char ptrOut = 3;
 unsigned char len = command.count() - 6; // von der Gesamtbytezahl eines Frames gehen 3 Bytes des Headers  ('#',Addr, Cmd) und 3 Bytes des Footers (1, 2, '\r') ab.

 while(len)
  {
   a = command.data()[ptrIn++] - '=';
   b = command.data()[ptrIn++] - '=';
   c = command.data()[ptrIn++] - '=';
   d = command.data()[ptrIn++] - '=';

   x = (a << 2) | (b >> 4);
   y = ((b & 0x0f) << 4) | (c >> 2);
   z = ((c & 0x03) << 6) | d;

   if(len--) command[ptrOut++] = x; else break;
   if(len--) command[ptrOut++] = y; else break;
   if(len--) command[ptrOut++] = z;	else break;
  }
 return command;

}

void * CBridge::Serial_Receiver_Thread(void* arg)
{
        CBridge *a = (CBridge * )arg;

        while(1)
        {
            a->readPendingSerialData();
            usleep (1000*2);
        }

        return NULL;
}

void * CBridge::UDP_Receiver_Thread(void* arg)
{

    CBridge *a = (CBridge * )arg;

    struct sockaddr_in si_me, si_other;
    int s, slen=sizeof(si_other);
    mavlink_status_t status;

    ssize_t len;

    if ((s=socket(AF_INET, SOCK_DGRAM, IPPROTO_UDP))==-1)
    {
        perror("socket");
        exit(1);

    }
    memset((char *) &si_me, 0, sizeof(si_me));
    si_me.sin_family = AF_INET;
    si_me.sin_port = htons(a->receiver_port);
    si_me.sin_addr.s_addr = htonl(INADDR_ANY);
    if (bind(s, (struct sockaddr *)&si_me, sizeof(si_me))==-1)
    {
        perror("bind");
        exit(1);

    }
    while(1)
    {
        if ((len = recvfrom(s, a->udp_rx_buffer, MAX_RX_BUFFER, 0,(struct sockaddr *)&si_other,(socklen_t *) &slen))==-1)
        {
            perror("recvfrom");
            exit(1);

        }

       for (int i = 0 ; i < len ; i++)
       {
           char inByte = a->udp_rx_buffer[i];
           if(mavlink_parse_char(MAVLINK_COMM_0, inByte, &a->mavlink_msg_from_qgc, &status))
           {

               //qDebug() << "\r# Received MAVLink message from QGC. MSG_ID: " << a->mavlink_msg_from_qgc.msgid;
               //fflush(stdout);
               //std::cout << "MK_QGC (" << CurrentUARTAddress << ")>>";
               //fflush(stdout);

               a->QGC2MK (a->mavlink_msg_from_qgc);
           }
       }
    usleep(1000*2);
    }

    close(s);
    return NULL;
}

void * CBridge::HeartBeat_Thread(void* arg)
{
        CBridge *a = (CBridge * )arg;
        while(1)

        {
            a->Timer_Heartbeat_Tick ();
            for (int i = 0 ; i < 5 ; i++)
            {
                if (a->CurrentUARTAddress == FC_ADDRESS)
                    a->request_MK_PPM_Channels();
                usleep(1000*200);
            }
 //           usleep (1000*100);
        }

        return NULL;
}

void CBridge::PrintLoss()
{

    qDebug() << "# Successful packets count: " << CRC_ok;
    qDebug() << "# Fail packets count:       " << CRC_fail;
    qDebug() << "# Packet loss rate:         " << (int)((float)CRC_fail / ((float)CRC_fail+(float)CRC_ok) * 100) << "%";
    fflush(stdout);
    std::cout << "MK_QGC (" << CurrentUARTAddress << ")>>";
    fflush(stdout);
}

void CBridge::PrintFlags(uint8_t FCStatusFlags, uint8_t FCStatusFlags2, uint8_t NCFlags)
{

    if (EnablePrintFlags)
    {
        qDebug() << "\r# FCStatusFlags: ";
        qDebug() << "\r#\t bit 0, (FC_STATUS_MOTOR_RUN):               "    << ((FCStatusFlags & 0x01) != 0);
        qDebug() << "\r#\t bit 1, (FC_STATUS_FLY):                     "    << ((FCStatusFlags & 0x02) != 0);
        qDebug() << "\r#\t bit 2, (FC_STATUS_CALIBRATE):               "    << ((FCStatusFlags & 0x04) != 0);
        qDebug() << "\r#\t bit 3, (FC_STATUS_START):                   "    << ((FCStatusFlags & 0x08) != 0);
        qDebug() << "\r#\t bit 4, (FC_STATUS_EMERGENCY_LANDING):       "    << ((FCStatusFlags & 0x10) != 0);
        qDebug() << "\r#\t bit 5, (FC_STATUS_LOWBAT):                  "    << ((FCStatusFlags & 0x20) != 0);
        qDebug() << "\r#\t bit 6, (FC_STATUS_VARIO_TRIM_UP):           "    << ((FCStatusFlags & 0x40) != 0);
        qDebug() << "\r#\t bit 7, (FC_STATUS_VARIO_TRIM_DOWN):         "    << ((FCStatusFlags & 0x80) != 0);

        qDebug() << "\r# FCStatusFlags2: ";
        qDebug() << "\r#\t bit 0, (FC_STATUS2_CAREFREE):               "    << ((FCStatusFlags2 & 0x01) != 0);
        qDebug() << "\r#\t bit 1, (FC_STATUS2_ALTITUDE_CONTROL):       "    << ((FCStatusFlags2 & 0x02) != 0);
    //    qDebug() << "\r#\t bit 2, (UNKNOWN): "    << ((FCStatusFlags2 & 0x04) != 0);
    //    qDebug() << "\r#\t bit 3, (UNKNOWN): "    << ((FCStatusFlags2 & 0x08) != 0);
    //    qDebug() << "\r#\t bit 4, (UNKNOWN): "    << ((FCStatusFlags2 & 0x10) != 0);
    //    qDebug() << "\r#\t bit 5, (UNKNOWN): "    << ((FCStatusFlags2 & 0x20) != 0);
    //    qDebug() << "\r#\t bit 6, (UNKNOWN): "    << ((FCStatusFlags2 & 0x40) != 0);
    //    qDebug() << "\r#\t bit 7, (UNKNOWN): "    << ((FCStatusFlags2 & 0x80) != 0);

        qDebug() << "\r# NCFlags: ";
        qDebug() << "\r#\t bit 0, (NCERR_FLAG_FC_COMPATIBLE):          "    << ((NCFlags & 0x01) != 0);
        qDebug() << "\r#\t bit 1, (NCERR_FLAG_MK3MAG_COMPATIBLE):      "    << ((NCFlags & 0x02) != 0);
        qDebug() << "\r#\t bit 2, (NCERR_FLAG_FC_COMMUNICATION):       "    << ((NCFlags & 0x04) != 0);
        qDebug() << "\r#\t bit 3, (NCERR_FLAG_MK3MAG_COMMUNICATION):   "    << ((NCFlags & 0x08) != 0);
        qDebug() << "\r#\t bit 4, (NCERR_FLAG_MKGPS_COMMUNICATION):    "    << ((NCFlags & 0x10) != 0);
        qDebug() << "\r#\t bit 5, (NCERR_FLAG_BAD_COMPASS_HEADING):    "    << ((NCFlags & 0x20) != 0);
        qDebug() << "\r#\t bit 6, (NCERR_FLAG_RC_SIGNAL_LOST):         "    << ((NCFlags & 0x40) != 0);
        qDebug() << "\r#\t bit 7, (NCERR_FLAG_EEPROM_NOT_FOUND):       "    << ((NCFlags & 0x80) != 0);

        fflush(stdout);
        std::cout << "MK_QGC (" << CurrentUARTAddress << ")>>";
        fflush(stdout);

    }

//    int len;
//    //TODO- check why this "ERROR" is always true ???
//    if (NCFlags & NCERR_FLAG_FC_COMPATIBLE)
//    {
//        qDebug()<<"\r# Error alert: NCERR_FLAG_FC_COMPATIBLE = " << NCERR_FLAG_FC_COMPATIBLE;

//        char error_text_msg[50] = "# Error alert: NCERR_FLAG_FC_COMPATIBLE";
//        mavlink_msg_statustext_pack(SYSTEM_ID,MAV_COMP_ID_SYSTEM_CONTROL,&mavlink_msg_to_qgc,CRITICAL_FAULT,(const int8_t*)error_text_msg);
//        len = mavlink_msg_to_send_buffer((uint8_t*)&udp_tx_buffer, &mavlink_msg_to_qgc);
//        if (sendto(socket_id, udp_tx_buffer, len, 0, (sockaddr *)&si_other, (socklen_t)  slen)==-1)
//         {
//             perror("sendto()");
//             exit(1);
//         }
//        fflush(stdout);
//        std::cout << "MK_QGC (" << CurrentUARTAddress << ")>>";
//        fflush(stdout);

//    }

//    if (NCFlags & NCERR_FLAG_MK3MAG_COMPATIBLE)
//    {
//        qDebug()<<"\r# Error alert: NCERR_FLAG_MK3MAG_COMPATIBLE = "<< NCERR_FLAG_MK3MAG_COMPATIBLE;

//        char error_text_msg[50] = "# Error alert: NCERR_FLAG_MK3MAG_COMPATIBLE";
//        mavlink_msg_statustext_pack(SYSTEM_ID,MAV_COMP_ID_SYSTEM_CONTROL,&mavlink_msg_to_qgc,CRITICAL_FAULT,(const int8_t*)error_text_msg);
//        len = mavlink_msg_to_send_buffer((uint8_t*)&udp_tx_buffer, &mavlink_msg_to_qgc);
//        if (sendto(socket_id, udp_tx_buffer, len, 0, (sockaddr *)&si_other, (socklen_t)  slen)==-1)
//         {
//             perror("sendto()");
//             exit(1);
//         }
//        fflush(stdout);
//        std::cout << "MK_QGC (" << CurrentUARTAddress << ")>>";
//        fflush(stdout);

//    }
//    if (NCFlags & NCERR_FLAG_FC_COMMUNICATION)
//    {
//        qDebug()<<"\r# Error alert: NCERR_FLAG_FC_COMMUNICATION = "<< NCERR_FLAG_FC_COMMUNICATION;

//        char error_text_msg[50] = "# Error alert: NCERR_FLAG_FC_COMMUNICATION";
//        mavlink_msg_statustext_pack(SYSTEM_ID,MAV_COMP_ID_SYSTEM_CONTROL,&mavlink_msg_to_qgc,CRITICAL_FAULT,(const int8_t*)error_text_msg);
//        len = mavlink_msg_to_send_buffer((uint8_t*)&udp_tx_buffer, &mavlink_msg_to_qgc);
//        if (sendto(socket_id, udp_tx_buffer, len, 0, (sockaddr *)&si_other, (socklen_t)  slen)==-1)
//         {
//             perror("sendto()");
//             exit(1);
//         }
//        fflush(stdout);
//        std::cout << "MK_QGC (" << CurrentUARTAddress << ")>>";
//        fflush(stdout);

//    }
//    if (NCFlags & NCERR_FLAG_MK3MAG_COMMUNICATION)
//    {
//        qDebug()<<"\r# Error alert: NCERR_FLAG_MK3MAG_COMMUNICATION = "<< NCERR_FLAG_MK3MAG_COMMUNICATION;

//        char error_text_msg[50] = "# Error alert: NCERR_FLAG_MK3MAG_COMMUNICATION";
//        mavlink_msg_statustext_pack(SYSTEM_ID,MAV_COMP_ID_SYSTEM_CONTROL,&mavlink_msg_to_qgc,CRITICAL_FAULT,(const int8_t*)error_text_msg);
//        len = mavlink_msg_to_send_buffer((uint8_t*)&udp_tx_buffer, &mavlink_msg_to_qgc);
//        if (sendto(socket_id, udp_tx_buffer, len, 0, (sockaddr *)&si_other, (socklen_t)  slen)==-1)
//         {
//             perror("sendto()");
//             exit(1);
//         }
//        fflush(stdout);
//        std::cout << "MK_QGC (" << CurrentUARTAddress << ")>>";
//        fflush(stdout);

//    }
//    if (NCFlags & NCERR_FLAG_MKGPS_COMMUNICATION)
//    {
//        qDebug()<<"\r# Error alert: NCERR_FLAG_MKGPS_COMMUNICATION = "<< NCERR_FLAG_MKGPS_COMMUNICATION;

//        char error_text_msg[50] = "# Error alert: NCERR_FLAG_MKGPS_COMMUNICATION";
//        mavlink_msg_statustext_pack(SYSTEM_ID,MAV_COMP_ID_SYSTEM_CONTROL,&mavlink_msg_to_qgc,CRITICAL_FAULT,(const int8_t*)error_text_msg);
//        len = mavlink_msg_to_send_buffer((uint8_t*)&udp_tx_buffer, &mavlink_msg_to_qgc);
//        if (sendto(socket_id, udp_tx_buffer, len, 0, (sockaddr *)&si_other, (socklen_t)  slen)==-1)
//         {
//             perror("sendto()");
//             exit(1);
//         }
//        fflush(stdout);
//        std::cout << "MK_QGC (" << CurrentUARTAddress << ")>>";
//        fflush(stdout);

//    }
//    if (NCFlags & NCERR_FLAG_BAD_COMPASS_HEADING)
//    {
//        qDebug()<<"\r# Error alert: NCERR_FLAG_BAD_COMPASS_HEADING = "<< NCERR_FLAG_BAD_COMPASS_HEADING;

//        char error_text_msg[50] = "# Error alert: NCERR_FLAG_BAD_COMPASS_HEADING";
//        mavlink_msg_statustext_pack(SYSTEM_ID,MAV_COMP_ID_SYSTEM_CONTROL,&mavlink_msg_to_qgc,CRITICAL_FAULT,(const int8_t*)error_text_msg);
//        len = mavlink_msg_to_send_buffer((uint8_t*)&udp_tx_buffer, &mavlink_msg_to_qgc);
//        if (sendto(socket_id, udp_tx_buffer, len, 0, (sockaddr *)&si_other, (socklen_t)  slen)==-1)
//         {
//             perror("sendto()");
//             exit(1);
//         }
//        fflush(stdout);
//        std::cout << "MK_QGC (" << CurrentUARTAddress << ")>>";
//        fflush(stdout);

//    }
//    // TODO- this ERROR is not  RC_SIGNAL LOST-
//    if (NCFlags & NCERR_FLAG_RC_SIGNAL_LOST)
//    {
//        qDebug()<<"\r# Error alert: NCERR_FLAG_RC_SIGNAL_LOST = "<< NCERR_FLAG_RC_SIGNAL_LOST;

//        char error_text_msg[50] = " Error alert: NCERR_FLAG_RC_SIGNAL_LOST";
//        mavlink_msg_statustext_pack(SYSTEM_ID,MAV_COMP_ID_SYSTEM_CONTROL,&mavlink_msg_to_qgc,CRITICAL_FAULT,(const int8_t*)error_text_msg);
//        len = mavlink_msg_to_send_buffer((uint8_t*)&udp_tx_buffer, &mavlink_msg_to_qgc);
//        if (sendto(socket_id, udp_tx_buffer, len, 0, (sockaddr *)&si_other, (socklen_t)  slen)==-1)
//         {
//             perror("sendto()");
//             exit(1);
//         }
//        fflush(stdout);
//        std::cout << "MK_QGC (" << CurrentUARTAddress << ")>>";
//        fflush(stdout);

//    }
//    if (NCFlags & NCERR_FLAG_EEPROM_NOT_FOUND)
//    {
//        qDebug()<<"\r# Error alert: NCERR_FLAG_EEPROM_NOT_FOUND = "<< NCERR_FLAG_EEPROM_NOT_FOUND;

//        char error_text_msg[50] = "# Error alert: NCERR_FLAG_EEPROM_NOT_FOUND";
//        mavlink_msg_statustext_pack(SYSTEM_ID,MAV_COMP_ID_SYSTEM_CONTROL,&mavlink_msg_to_qgc,CRITICAL_FAULT,(const int8_t*)error_text_msg);
//        len = mavlink_msg_to_send_buffer((uint8_t*)&udp_tx_buffer, &mavlink_msg_to_qgc);
//        if (sendto(socket_id, udp_tx_buffer, len, 0, (sockaddr *)&si_other, (socklen_t)  slen)==-1)
//         {
//             perror("sendto()");
//             exit(1);
//         }
//        fflush(stdout);
//        std::cout << "MK_QGC (" << CurrentUARTAddress << ")>>";
//        fflush(stdout);

//    }

}

